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ABSTRACT. An efficient Lagrangian formulation of manipulator dynamics has been developed. The 
efficiency derives from recurrence relations for the velocities, accelerations, and generalized forces. The num- 
ber of additions and multiplications varies linearly with die number of joints, as opposed to past Lagrangian 
dynamics formulations with an n 4 dependence. With this formulation it should be possible in principle to 
compute the Lagrangian dynamics in real time. The computational complexities of this and other dynamics 
formulations including recent Ncwton-Euler formulations and tabular formulations are compared. It is con- 
cluded that recursive formulations based either on the Lagrangian or Newton-Euler dynamics offer the best 
method of dynamics calculation. 
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Introduction 

The inverse problem for manipulator dynamics, namely the problem of computing die joint torques 
required to produce given joint positions, velocities, and accelerations, has until recently been a computational 
bottleneck in the control of manipulators. Past formulations of manipulator dynamics have been based on a 
relatively inefficient derivation from the Lagrange equations [1,2]. The solutions to these equations required on 
the order of seconds on a computer for each trajectory point, and it was perceived that therefore torques for 
trajectories could not be computed in real time. 

Two different types of schemes were proposed to render the Lagrangian dynamics computationally 
feasible: (1) simplifying the dynamics by ignoring some terms and correcting errors with feedback, and (2) 
tcssellating the state space along various dimensions and tabulating part of the solution. The most common 
method of simplifying the dynamics has been to ignore the Coriolis and centrifugal forces, which are by far the 
greatest computational burden in the dynamics calculation [3,4]. However, ignoring Coriolis and centrifugal 
forces works well only at slower speeds of movement; at fast speeds of movement the Coriolis and centrifugal 
forces are a major component of the dynamics [5]. The errors in the computed torque resulting from ignoring 
Coriolis and centrifugal forces cannot be corrected with feedback because of excessive requirements on the 
corrective torques [6]. 

Tabular solutions to the dynamics seek to achieve economies in computation time at the expense of large 
memories. Raibert [7] has discussed this space-time tradeoff and has examined the different dimensions along 
which the tabularization can be set up. Albus [8,9] represents the space extreme by including in his table all 
dimensions (q, q, q), representing n dimensional position, velocity, and acceleration vectors. Since the general- 
ized forces have the form Fi = f(q, q, q), these forces are obtained at no computational cost by indexing the 
table with die desired trajectory values (q, q t q). Raibert [10] eliminated the acceleration dimension from die 
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3 Recursive Lagrangian Dynamics 

table by expressing the dynamics in the form F z = J(q)q -f K{q, q) and tabulating the position and velocity 
dependent terms J(q) and K(q, q). Because of the large memory costs involved in the above two methods, 
Horn and Raibcrt [6] proposed a configuration space control that tabulates only the position dependent terms. 
Their formulation will be presented later in conjunction with a discussion of its computational complexity. 
The difficulties with tabular methods include (1) enormous table sizes necessitated by creating a fine enough 
tessellation along the various dimensions, (2) filling the entries in the table, (3) interpolating between stored 
values, and (4) the tables become useless if the load changes suddenly such as when an object is picked up. 

This paper presents a reformulation of the Lagrangian dynamics for a manipulator that greatly reduces 
the computational burden and that should allow real time computation of the inverse dynamics. Specifically, 
tiiere are three parts to the reformulation: 

1. backward recursion of the velocities and accelerations working from the base of the manipu- 
lator to the end link, 

2. forward recursion of the generalized forces working from the end link to die base of the 
manipulator, and 

3. use of 3x3 rotation matrices instead of 4x4 rotation-translation matrices and homogeneous 
coordinates. 

The end result of applying these methods is that the number of additions and multiplications varies linearly 
with the number of joints. This reformulation brings the Lagrangian dynamics into line with recently 
developed recursive Newton-Ruler dynamics [5,11,12], whose computational complexity varies linearly with 
the number of joints and whose formulation contains parts 1 and 2 above. Part 3 was delineated in [12] for 
the Newton-Euler metfiod, and a similar preference for 3x3 versus 4x4 matrices was expressed for reasons of 
computational efficiency. 

The Uickcr/Kuhn Lagrangian Formulation 

The standard formulation for manipulator dynamics is derived from Uickcr [1], who used 4x4 matrices to 
set up the Lagrangian based dynamics for a somewhat more general linkage problem. This formulation was 
particularized to open loop kinematic chains by Kahn [2]. Borrowing Kahn's notation (Figure 1), the links of 
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Figure I. Standard coordinate axes definitions for connected links and relationships between 
neighboring coordinate axes. 



a manipulator arc numbered consecutively from 1 to n starting From die base to the tip. Ry convention the 
reference frame is numbered link 0. The joints are the points of articulation between links, and are numbered 
so that joint i connects links i — 1 and i. An orthogonal coordinate system is fixed in each link as follows: 

z i is directed along the axis of joint i -f 1, 

x i lies along the common normal from zi—\ to z. u and 

y* completes the right handed coordinate. 

The relative position of two adjacent links is completely described by the following four parameters: 

a i is the distance between the origins of coordinate systems i — 1 and i measured along x u 
s i is the distance between x 2 _i and x t measured along ^-__i, 

Q i is the angle between die ^_i and z % axes measured in a righthand sense about x u and 
6i is the angle between the ^_j and the x { axes measured in the righthand sense about j%_i. 

If the joint is rotational die joint variable will be 9 t \ if translational the joint variable will be s t . The symbol 
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qi will designate the variable for joint i whether it is s t or im The vector Q = (q it q 2) . . ., q n ) represents the 
generalized coordinates of the manipulator and completely specifies the position. Multiple rotational degrees 
of freedom can be expressed by joints with zero mass and zero length links. 

Let l Vj = (1 x y z) T be a vector from coordinate system i to a point fixed in link j expressed in link i 
coordinates. Then adjacent coordinate systems are related by die relation: 



i—\ 



vj = Ai l Vj 



(1) 



where A t is the 4x4 transformation matrix between coordinate systems i — 1 and i: 

"10 

ai cos Q{ cos 9i — sin #i cos a; sin t sin a; 
f ^^ CLi&mQi sm6i cos 0* cos a* — cos ^ sin a; 

. St sin a z cosa^ 

We define A = J, the identity matrix. Any two coordinate systems i and j can be related by cascading the 
transformations: 



(2) 
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(3) 



where l Wj = A i+ iA i+2 - ■ A' for i < j. We define j W 3 - = I. When referring to the base coordinate system 
we omit die superscript, so that v k — °v k and Wj = °Wj. 

Using tli is notation, the generalized forces F { for an n-link manipulator have been derived from die 
Lagrange equations [1,2]: 



r\ 



E 






Jfe=l 



(4) 



^mm^ 



Recursive Lagrangian Dynamics 



where 



Jj is die inertia tensor with respect to the proximal joint of link j expressed in j's body coor- 
dinates, 

171 j is the mass of link j, 

is the gravity vector, and 

3 r . is the coordinate of the center of mass of link j expressed in link fs body coordinates. 
Evaluation of (4) has been considered too slow for real time computation. As an estimate of the complexity 
of computing the dynamics in this formulation, the total number of additions and multiplications has been 
calculated. In arriving at a number, die operations in (4) were carried out more or less as set forth, because any 
efficiencies in calculating these terms should be explicit in the formulation rather than hidden in a program. A 
few reasonable economies nevertheless were practiced. Simple decompositions saved some rccomputation. 

<% dq k 

p^=W k J-^>W^f-% k<l<j (5) 

dq k dq dq k dq 






=w k - 1 -^f k w j fc = 



j****k 



The terms k Wj were computed first and stored. Then thcdWj/dq k and d 2 Wj/dq k dq terms were computed and 
their results also stored. The cost of computing the terms in the matrices A^dAjjdq^ and^A//^ was not 
included, since they depend on the particular manipulator configuration and because they contribute only a 
small linear cost to the computational complexity. Finally, no allowance has been made for any sparseness of 
the matrices. 

Table I shows that the Uickcr/Kahn formulation gives an n 4 dependence on the number of multiplica- 
tions and on the number of additions. Table II shows that for n = 6 diese numbers arc 66,271 and 51,548 
respectively. These large numbers of operations are too time consuming to compute in real time. Lull et al. [5] 
have estimated that to compute die torques for one nominal point in the trajectory requires 7.9 seconds on a 
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Table I. Comparison of Dynamics Formulations 


Method 


Multiplications 


Additions 


Uicker/Kahn 


32+n l + 86 5/l2n 3 + 171|n 2 
+53 ^n- 128 


25n' 1 + 66 |n 3 + 129 \n % 
+42 in — 96 


Waters 


106 in 2 + 620 ^n- 512 


82n 2 + 514n — 384 


Hollcrbach(4x4) 


830n — 592 


675n — 464 


Hollcrbach(3x3) 


412n — 277 


320n — 201 


Ncwton-Tailcr 


150n — 48 


131n — 48 


Horn, Raibert 


2n :i + n 2 


n 3 + n 2 + 2n 



Table 11. Comparison for rc = 6 


Method 


Multiplications 


Additions 


Uicker/Kahn 


66,271 


51,548 


Waters 


7,051 


5,652 


Hollcrbach(4x4) 


4,388 


3,586 


1 lo11crbach(3x3) 


2,195 


1,719 


Ncwton-Euler 


852 


738 


Horn, Raibert 


468 


264 



POP 11/45. 

Lagrangian Dynamics with Backward Recursion 

Waters [13] first noticed that the generalized forces (4) could be expressed in a form that allowed one 
to take advantage of several backward recurrence relations (backward in the sense of die link numbering direc- 
tion). We present below a simpler formulation and derivation of these recurrence relations. In reexamining the 
derivation of the generalized forces it becomes apparent that the generalized forces (4) can be expressed in a 
more compact form: 
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dm J 



l,...,n 



(6) 



where Wj could be expanded to yield the iisual form for the components of the reaction and the Coriolis and 
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centrifugal forces: 



J , SW- j j r) 2 W 

"*=£**+£x££« (7) 



It is however best to leave this term unexpanded. With this more compact formulation the specific recurrence 
relations for the velocities Wj and accelerations Wj arc easily derived by straightforward differentiation: 

W j =W j . l A j (8) 



dA 
l dqj 



= W j ^A J + W J ^q j (9) 



W, = W^ + 1V,^ + W^ 4j + M,-, ^g), + HS-^% 

- fy-M, + 2^_,^. + Wj-i^fj + ^-i^% (10) 

With this formulation the number of additions and multiplications is reduced to an n 2 dependence 
(Tables I and II). For n = 6 there are 7051 multiplications and 5652 additions. The reduction from an 
n 4 to an n 2 dependence comes about primarily from a more efficient Coriolis and centrifugal force computa- 
tion; that is to say, this formulation requires only calculation of d^W./dq] instead of all of the matrices 
dHVj/d qk d qi . 

Forward Recursive Lagrangian Dynamics 

An additional level of forward recursion can be placed on the generalized force formulation (6), leading 
to further efficiency of the Lagrangian dynamics. We note that dWj/dqi = dWijdqi { Wj. The generalized 
force equation (6) can then be written: 
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(11) 



Phis reformulation lends itself to forward recursion as follows: 



Di = 2 WW 



j=t 



/^"""n 



j=i+l 

= JiW* + A i+ iD i+1 



(12) 



Ci = J2 m i iW J Jr i 



3=1 



= m,- Vi + /i t - +1 c i+ i 



(13) 



Substituting into (11), the generalized forces become simply: 
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(14) 



r^- 



This recursive formulation is computed similarly to recursive formulations of the Ncwton-Eulcr dynamics 
[5,1 1,12]. First all the W { terms arc computed staring from i = 1 to i — n using the recurrence relations (6). 
Then the A' and c t - terms are computed in the other direction, from i = n to i = 1. With this formulation 
there are now 830n — 592 multiplications and 675n — 464 additions; for n = 6 there are 4388 multiplica- 
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Recursive lagrangian Dynamics 10 

tions and 3586 additions. This linear computation cost has essentially been obtained for free, eliminating the 
n 2 cost in Waters' formulation without significantly changing the coefficients of the other terms. 

Recursive Lagrangian Dynamics with 3x3 Matrices 

Since the previous formulation reduced the computational cost to an n dependence, all that can be done 
is to reduce the size of the coefficients. The greatest reduction can be obtained through a reformulation of the 
Lagrangian dynamics in terms of 3x3 matrices rather than 4x4 rotation-translation matrices. Although more 
convenient in setting up the dynamics, 4x4 matrices are inefficient because of some sparseness and because 
of the combination of translation with rotation (see e.g. [12]). While it is possible to have special purpose 
multiplication routines which take into account the zeros and ones of the last row of the 4x4 transformation 
matrices, it has consistently been the position of this paper that economies in computation should be explicit 
in die formulation rather than implicit in the programming. Because 3x3 matrix multiplications require 
27 multiplications while 4x4 matrix multiplications require 64, we get a greater than 50% reduction in the 
coefficients of the computational cost terms. 

We suppose that A, now represents a 3x3 rotation matrix relating the orientations of coordinate system 
j — 1 and j. That is to say, if 3 v is a vector expressed in terms of the orientation of coordinate system j 
axes,then J ~ l v = Aj J v. When a vector is presented without a left superscript, it is referred to die base 
coordinate orientation (v = °v). In the subsequent development, capital letters represent 3x3 matrices, lower 
case letters represent 3x1 vectors. j W k is defined as before, save that it is now the composition of 3x3 rotation 
matrices. Define the following vectors (see Figure 2): 

Pi is a vector from the base coordinate origin to the joint i coordinate origin, 
p* is a vector from coordinate origin i — 1 to coordinate origin i, 
r i is a vector from the base coordinate origin to the link i center of mass, 
r * is a vector from coordinate origin i to the link i center of mass, and 
is r*/mi. 
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link i 
center of mass 



link i-1 
origin 




base 



link i 
origin 



or 



igm 



Figure 2. Vector definitions between the base origin and the link origins and center of masses. 



The generalized forces for this system are derived in Appendix A: 
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(15) 



The Wj term lias the same recursive expression as in equation (10), though presently referring to a 3x3 rotation 
matrix. Thep, term has the following recurrence relations: 



Pj = Pj-i ~ W^p*j 



(16) 



Lastly, in Appendix B the forward recursive relation for A is derived. The recurrence relation for c { is the 
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same as (13). 
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where 



^r ■ ~~t 



Di =Ai+iDi+i + l p i+ iei+i + l n{p i + JiW. 
*i =^+1 + mipj + l nfW i 



(18) 



This formulation decreases the coefficients of the complexity polynomial of the recursive Lagrangian formula- 
tion with 4x4 matrices by better than 50% (Tables I and II). For n = 6 there are now 2195 multiplications and 
1719 additions. 

Recursive Newton-Euler Dynamics 

The Lagrange equations represent only one of several possible approaches towards deriving the dynamic 
equations for open loop kinematic chains. Other approaches include Newton-Buler formulations [5,11,12,14,- 
15,16,17,18], a Lagrangian formulation of D'Alembcrt's Principle [19,20], and virtual work formulations [21]. 
Various dynamics formulations have been contrasted and compared in [22]. 

Recently a number of papers have appeared on an efficient recursive Newton-Euler formulation of 
manipulator dynamics [5,11,12], Hooker and Margulies [14] and Hooker [15] presented an early application of 
Newton-Ruler equations to open-loop kinematic chains such as satellites. Stcpancnko and Vukobratovic [16] 
developed a recursive Newton-pAilcr formulation in the context of human limb dynamics. This formulation 
was revised and elaborated in subsequent papers [17,18]. 

We present a revision of the Ncwton-Eailer equations as presented in [5]. The backward recursion 
propagates angular velocities, angular accelerations, linear accelerations, total link forces, and total link 
torques from the base to die end link. For a rotational joint, the recursive equations are: 
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&i =Ui—\ + Zi-\qi + ^-l X Zi-\4i 
Pi =di Xp- + u)i X {u)i X p-) + Pi__ x 
r t =u) t X (wi X r ■ ) + Wi X r* + P; 
F* =ra/r ? - 
Nt =IiWi + WiX (/^) 

where previously undefined terms are: 

^' is the angular velocity of link t, 
u)i is the angular acceleration of link i, 
Fi is the total external force on link i, 
f^ A/* is the total external torque on link i, and 

A is the inertia tensor of link i about its center of mass. 

The forward recursion propagates the forces and moments exerted on link i by link i — 1 from the end link of 
the manipulator to the base, 

fi =^i* -\~ fi+l 

m =n i+ i + Ni + (p* + r*) X F t - + p* X JJ+i 
^ =Zi—i * n* 



where 



JS is the force exerted on link i by link i — I, 

n i is the moment exerted on link i by link i — 1, and 

r *' is the input torque at joint i. 



^ m \. 



In this formulation the implicit reference coordinate frame is die base coordinates. In the next section a more 
efficient reference frame is examined. 



^**s 



^*N 



Recursive Lagrangian Dynamics 14 

Recursive Newton-Euler Dynamics Referred to Link Coordinates 

Orin ct al. [12] initially proposed that the forces and moments in the Newton-Euler formulation of [16] 
be referred to the link's internal coordinate system. Armstrong [11] and Luh ct al. [5] extended this idea by 
calculating the angular and linear velocities and accelerations in link coordinates as well. The end result of 
referencing both the dynamics and kinematics to the link cordinates is to obviate a great deal of coordinate 
transformation and to allow the inertia tensor to be fixed in each link coordinate system. 

Although Armstrong's and Luh's formulations are equivalent, they addressed complementary problems. 
Armstrong sought to integrate the dynamic equations to find the accelerations, given the applied torques. 
Luh solved the inverse problem, namely to find the appropriate joint torques given desired joint positions, 
velocities, and accelerations. Another difference is that Armstrong's dynamics arc referred to coordinate sys- 
tems located at the joints, while Luh's dynamics are referred to coordinate systems located at the link centers 
of mass. 

Using Luh's formulation once again, and rather than rewriting all the Newton-Euler equations showing 
how they arc referred to internal link coordinates, we present 3 examples of this reformulation. For a more 
complete presentation, the reader is referred to [5]. 

•^ =/in f - i ^i— i + 

% =%% + U x (*IM) 

i fi= i F i +A i+l ( i + l f i+i ) 

where Aj = [Ai)- 1 , and J v denotes a vector v represented in the orientation of coordinate system j 
measured from the base origin. From Tables I and II it is seen that the recursive Newton-Euler formulation 
gives a 60% reduction in additions and multiplications over the recursive Lagrangian formulation. For n = 6 
there arc only 852 multiplications and 738 additions. 
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The Configuration Space Method 

The Configuration Space Method [6] is derived from the following reformulation of the Lagrangian 
dynamics: 

n n n 

Fi = Gi{q) + J2 ^(^ + £ E Cy**/* 

The gravity compensation G„ inertial terms J u , and Coriolis coefficients Q j/c are tabulated in tliis 
formulation. The number of additions and multiplications using the Configuration Space Method are 
presented in Tables I and II. Although there is still an n 3 dependence for the additions and multiplications, 
the much smaller coefficients on the polynomial terms represent a greatly reduced computational cost. For 
n = 6 there are just 468 multiplications and 264 additions. An additional saving not indicated in these figures 
is that it is unnecessary to compute the sines and cosines of angles. However, for n > 9 die Configuration 
Space Method becomes less efficient than the Newton-Euler formulation. 

Conclusions 

The problem of computing manipulator dynamics in real time appears to have been solved. With both 
the recursive Lagrangian formulation and the recursive Newtonian formulation of the dynamics, the number 
of additions and multiplications varies linearly with the number of joints. For 6 joints the number of additions 
and multiplications should be computable in real time with even modest computers such as PDPlTs. The 
impetus for applying approximations or tabular techniques to solve the dynamics appears to have been lost. 
With respect to the Configuration Space Method in particular, which for n < 9 joints is the most efficient 
formulation, problems incumbent with the use of tables such as large memories, configuration sensitivity, 
interpolation, and filling the tables are obviated, 

It appears unlikely that any but very minor improvements in efficiency can be made to the present recur- 
sive formulations. The recursive Newton-Fuler formulation is more efficient than the recursive Lagrangian 
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formulation presented here. One reason for presenting this recursive Lagrangian formulation is to demonstrate 
that the Lagrangian formulation is not necessarily so computationally intensive as to preclude real time com- 
putation, as had been the assumption for the past 10 years [2,3,4,6], and to demonstrate that the Lagrangian 
formulation can be made roughly as efficient as the Newton-Euler formulation [5]. For some applications 
involving the use of homogenous coordinates and 4x4 transformation matrices, e.g. [23], the recursive 
Lagrangian formulation may be the most convenient efficient dynamics formulation. 
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Appendix A 



In this appendix the kinetic energy for a link is derived, and the Lagrange equations are solved. Let /if be 
a vector from the base origin to a point fixed in link i, /i* a vector from coordinate system i to the same point. 
Then 



h i =Pi + W i % (/U) 

h l =Pr + W i % (A2) 



The kinetic energy for a particle on link i is given by: 



)dm 



(A3) 



ki = -tr(hih[y 

/-N =\tr[pii> T i + 2VK- %p[ + Wi % % T Wj)dm 

Integrating over all particles in link i, the total kinetic energy K{ of link { is given by: 

Ki = -trfm&pj + 2Wt l n^[ + WiJiWj) {A4) 

where rrti is the mass of link t, l ni = f l h*dm and ^/m; is the vector to the center of mass, and J{ = 
/ l h\ Vi* dm is the inertia tensor with respect to the coordinate system {. The total kinetic energy for all the 
links is given by: 



n 



3=1 

(A5) 



1 

= 2 £ tr ( m JPJpJ + WjinjpJ + WjJjWj) 

3=1 
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Appendix A 20 

The potential energy derives from gravity and is equal to the sum of the work required to displace the mass 
center of each link from the horizontal reference plane: 

n 

P = P-J2 m ^ T Wri (A&) 

i=i 

where g — {g x g y g z ) is die acceleration due to gravity and P is a constant which depends on die particular 
reference plane chosen. The Lagrangian L is defined as the difference between the kinetic and potential 
energies: L = K — P. The Lagrange equations are: 



„ d(dL\ SL . t 



(AT) 



Since the potential energy is position dependent only, 



' dt\dq\) <% + <% 



(A8) 



The terms are calculated below. 






dK 
<9Qi 



dP A JSWj , 



3=i 
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It can be shown that 





dqi 




dtydqi j 


dWj 
dqi 


dWj 

dq\ 


dWj 


_ d (dWj 



dq t dtXdqt J 
Substituting into the Lagrangian, and noting that tr(AB T ) = tr(BA T ) 9 



(,412) 



j#*\ 



«-E 



j=t L 






. VIS, rp 8PJ . rp » T dWj • T OWj -t\ r r 3Wj . 



<% 



% 



% 



(413) 
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In this appendix the forward recursive relations are developed. For i < j, 



Pj =Pi + Wi { pj 



jg*"**>r K 



dqi dqi 
Substituting into (15) and factoring, 



<% _dWi i 



(52) 
The terms inside the summations may be computed by recurrence relations. 

j = i ^ 

f; ((* +1 •+ i Pi + Wi)(m y pj + M^f) + ^-+i i+ * ^( s# + -wJ)) + '"*r + Ji- wJ 



=y4 i+1 A+i + l Pi+ie i+ i + *n,#' + .W 



T . ......r 



(53) 
where 



j—i ^ ' 



(54) 
=e i+1 + mrf>T -f i n T i W i 
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The recurrence relation c 2 * for the gravity term is the same as equation (13), except for the dimensionality 
difference. Substituting into (B2), die generalized force is: 



y-s 



